// Modified from http://www.arduino.cc/playground/Main/BarebonesPIDForEspresso

float updatePID(float targetPosition, float currentPosition, float *lastPosition, float *iState, float *PID)
{
  // PID is an array where:
  // PID[0] = P Gain
  // PID[1] = I Gain
  // PID[2] = D Gain
  
  float error;
  float dTerm;

  error = targetPosition - currentPosition;
  
  *iState += error;
  if (*iState < -windupGuard) *iState = -windupGuard;
  else if (*iState > windupGuard) *iState = windupGuard;
  
  dTerm = PID[2] * (currentPosition - *lastPosition);
  *lastPosition = currentPosition;

  return (PID[0] * error) + (PID[1] * (*iState)) + dTerm;
}

void zeroIntegralError() {
  iRollState = 0;
  iPitchState = 0;
  iRollLevel = 0;
  iPitchLevel = 0;
  iYawState = 0;
}
